const int ledPin = 13;

void setup() {  
  pinMode(ledPin, OUTPUT);
  pinMode(6, INPUT);
  motorSetup();
  sensSetup();
  Serial.begin(115200);  
  Serial.println("WALL-E V2 says hello!");   
  blink(3);
}

void loop() {  
  //1=right; 2=MR; 3=ML; 4=Left; 5=BL; 6=BR;
   
  UpdateSpeed();
  NullEnc();
  
  if(sens()==0){
    forward();
  }
  else{
    stopp();
  }
  
  //Back sensors
  if(sens()==5){
    forward();
    driveFor(1);
    right();
    driveFor(1);
    while(sens()==5){
      right();
      ThingsToDoInWhile();
    }
  }
  
  if(sens()==6){
    forward();
    driveFor(1);
    left();
    driveFor(1);
    while(sens()==5){
      left();
      ThingsToDoInWhile();
    }
  }
  
  //Right sensors
  while(sens()==1){
    right();
    ThingsToDoInWhile();
  }

  if(sens()==2){
    shortback();
    left();
    driveFor(1);
    stopp();
    while(sens()==2){
      left();
      ThingsToDoInWhile();
    }
  } 

  //Left sensors
  while(sens()==4){
    left();
    ThingsToDoInWhile();
  } 
  
  if(sens()==3){
    shortback();
    right();
    driveFor(1);
    stopp();
    while(sens()==3){
      right();
      ThingsToDoInWhile();
    }
  }
}

void ThingsToDoInWhile(){
   driveFor(1);
   UpdateSpeed();
   if(getEncCount()>4){stopp();}  
}
void set(byte pin, byte value){
  digitalWrite(pin, value); 
}

void blink(int a) {
  for(int i=0;i<a;i++){ 
    set(ledPin, HIGH);   // set the LED on
    delay(70);              // wait for a second
    set(ledPin, LOW);    // set the LED off
    delay(230);              // wait for a second 
  }
}

String lastLog;
void logMsg(String msg) {
  if(msg!=lastLog){
    Serial.println(msg);  
    delay(1);
    lastLog = msg;
  }
}



